
#include <ros.h>
#include <geometry_msgs/Twist.h>
#define USE_USBCON
#define   m_WHEEL_D   150//m_WHEEL_D：两个主动轮之间的间距，单位mm
#define   m_WHEEL_R   60//m_WHEEL_R：主动轮的半径，单位mm
#define   LEFT        0
#define   RIGHT       1
#define   _PI          3.1415926
#define   m_PROPORTION      30

ros::NodeHandle  nh;

void cmd_vel_callback( const geometry_msgs::Twist& twist_aux){
    double vel_x = twist_aux.linear.x * 1000;    //m/s
    double vel_th = twist_aux.angular.z;   //1000rad/s
    double pv[2] = {0.0,0.0};
    double  pw[2];
    pv[LEFT] = (double)( vel_x - (vel_th * (double)m_WHEEL_D) / (double)2 );
    pv[RIGHT] = (double)( vel_x + (vel_th * (double)m_WHEEL_D) / (double)2 );
    for (int i = 0; i < 2; i++) {
      pw[i] = (double)(pv[i]/ (double)m_WHEEL_R);
    }
    int rpm_l = (int)((pw[LEFT]*(((double)30)*(double)m_PROPORTION))/_PI);
    int rpm_r = (int)((pw[RIGHT]*(((double)30)*(double)m_PROPORTION))/_PI);
    
    if(rpm_l>0) digitalWrite(4,LOW);
    else        digitalWrite(4,HIGH);
    analogWrite(5,fabs(rpm_l/3000)*255);
    if(rpm_r>0) digitalWrite(7,HIGH);
    else        digitalWrite(7,LOW);
    analogWrite(6,fabs(rpm_r/3000)*255);
}

ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("columba_velocity_controller/cmd_vel", &cmd_vel_callback);

void setup() {
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  nh.initNode();
  nh.subscribe(cmd_vel_sub);
}

void loop() {
  nh.spinOnce();
  delay(10);
}
